#ifndef SIMPLE_TP_H
#define SIMPLE_TP_H

// stopping criterion:
#define TINY_DP(max_acc, period) (max_acc * period * period * 0.001)

class simple_tp_t
{
public:
    double pos_cmd = 0;  /* position command */
    double max_vel = 0;  /* velocity limit */
    double max_acc = 0;  /* acceleration limit */
    int enable = 0;      /* if zero, motion stops ASAP */
    double curr_pos = 0; /* current position */
    double curr_vel = 0; /* current velocity */
    int active = 0;      /* non-zero if motion in progress */
    void reset(double pos)
    {
        pos_cmd = pos;
        max_vel = 0;
        max_acc = 0;
        enable = 0;
        curr_pos = pos;
        curr_vel = 0;
        active = 0;
    }
};

/* I could write a bunch of functions to read and write the first four
   structure members, and to read the last three, but that seems silly.
*/

/* The update() function does all the work.  If 'enable' is true, it
   computes a new value of 'curr_pos', which moves toward 'pos_cmd'
   while obeying the 'max_vel' and 'max_accel' limits.  It also sets
   'active' if movement is in progress, and clears it when motion
   stops at the commanded position.  The command or either of the
   limits can be changed at any time.  If 'enable' is false, it
   ramps the velocity to zero, then clears 'active' and sets
   'pos_cmd' to match 'curr_pos', to avoid motion the next time it
   is enabled.  'period' is the period between calls, in seconds.
*/

extern void simple_tp_update(simple_tp_t* tp, double period);

extern void simple_tp_deceleration(simple_tp_t* tp, double period);

/*
@brief do coarse plan for a single joint movement, with given resitrictions, (vs, ve are 0)
@param s[IN]    the increment in joint position
@param a_max[IN]    the maximum acc set for this movement
@param vp[IN]   the velocity specified
@param va[OUT]  the actual max velocity according to restrictions
@param t[OUT]   the time interval for all 3 stages
*/

extern void coarse_plan(double s, double a_max, double vp, double* va, double* t);

#endif /* SIMPLE_TP_H */
